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ABSTRACT 


In  a  tactical  missile  with  mid-course  guidance,  an 
Inertial  Navigation  System  (INS)  is  required.  Strapdown  INS 
using  Steady-State  Kalman  Filters  (SKF)  as  estimators  has 
been  suggested  and  this  kind  of  INS  is  considered  to  be 
easier  and  cheaper  to  implement  than  the  gimbaled  INS. 

This  thesis  investigates  one  aspect  of  the  INS  problem: 
The  sensitivity  of  the  SKF  to  inaccuracies  in  the  filter 
parameters  such  as  the  stability  derivatives.  The  analysis 
has  been  performed  by  varying  each  of  the  flight  parameters 
over  a  given  range  and  noting  the  effect  on  the  accuracy  of 
the  filter.  The  great  advantage  of  the  analysis  in  the 
sensitivity  of  rms  estimate  errors  to  inaccuracies  in  the 
stability  derivatives  is  that  it  points  out  clearly  which 
derivatives  are  worthy  of  detailed  accurate  determination  and 
which  are  not. 
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I .  INTRODUCTION 


In  a  typical  tactical  missile  trajectory,  standoff  (or 
mid-course)  guidance  is  required  when  the  missile  is  launched 
at  such  long  ranges  from  the  target  that  either  the  missile 
seeker  cannot  detect  the  target  or,  if  it  can,  the  information 
is  of  such  poor  quality  that  is  unusable.  The  mid-course 
guidance  law  usually  consists  of  some  pre-programed  strategy 
such  as  to  maintain  the  launch  heading,  constant  altitude 
and  speed.  Mid-course  guidance  is  primarily  an  inertial 
instrumentation  problem.  Typical  sensors  onboard  missiles 
consist  of  three  rate  gyros  (pitch,  yaw  and  roll),  accelero¬ 
meters,  sometimes  magnetic  compass  and  some  kind  of  altimeter. 
Additional  state  information  can  be  provided  by  the  seeker. 

A  radar  seeker  could  provide  range  and  range  rate. 

Here  is  considered  the  problem  of  using  steady-state 
Kalman  Filters  as  part  of  a  navigation  system,  where  the  two 
estimators  together  (longitudinal  and  lateral  estimators), 
constitute  a  strapdown  system  that  does  not  use  accelero¬ 
meters  or  gimbaled  gyros.  Strapdown  Inertial  Navigation 
Systems  (INS)  using  Steady-State  Kalman  Filters  (SKF)  as 
estimators  have  been  considered  as  cheaper  and  easier  to 
implement  than  the  gimbaled  INS.  The  sensors  used  in  this 
analysis  are:  pitch  and  roll  rate  gyros,  altimeter  and  mag¬ 
netic  compass.  Since  constant  gain  estimators  have  been 
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suggested  as  easier  and  cheaper  to  implement,  the  analysis  is 
confined  to  the  SKF. 

This  work  has  been  motivated  by  the  idea  given  in  Ref. 
[4],  where  the  author  discusses  in  detail  the  configuration 
of  such  a  navigation  system  as  described  before,  but  the 
problem  is  presented  as  a  particular  applicati;  .  in  the  INS 
of  the  DC-8  airplane.  For  convenience  the  model  used  here 
essentially  is  the  same  as  Ref.  C4]  in  lieu  of  a  missile. 

This  was  done  in  order  to  eliminate  classification  problems. 

This  thesis  investigates  only  one  aspect  of  the  INS 
problem.  That  aspect  is  the  sensitivity  of  the  Kalman  Filter 
to  inaccuracies  in  the  filter  parameters  or  variation  between 
the  filter  model  and  the  plant  model.  These  differences  can 
be  due  to  either  inaccuracies  as  indicated  or  to  a  normal 
variation  in  the  flight  parameters  due  to  different  flight 
regimes.  One  result  of  this  is  the  sensitivity  of  rms 
estimate  errors  to  inaccuracies  or  differences  in  the  sta¬ 
bility  derivatives. 

The  work  was  begun  by  reproducing  the  results  given  in 
Ref.  [4]]  with  the  correct  implementation  of  the  dynamics  in 
the  filter  parameters  (i.e.,  Xu,  Yv,  etc.)  over  a  given 
range  and  noting  the  effect  on  the  accuracy  of  the  filter. 
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II.  STOCHASTIC  MODELS  AND  ESTIMATION 
A.  KALMAN  FILTER 

The  Kalman  Filter  is  widely  documented  and  no  attempt  at 
a  development  of  general  theory  has  been  made  in  this  work. 

A  brief  description  has  been  included  in  order  to  establish 
the  particular  formulation  used.  For  a  more  complete  de¬ 
velopment  one  is  referred  to  Ref  [ID* 

1 .  Linear  Dynamic  System 

The  formulation  used  employs  state-space  notation 
which  offers  the  advantage  of  mathematical  and  notational 
convenience.  Consider  the  linear  time  invariant  system 
(system  and  measurement  models)  given  by  equation  (1),  where 
x  represents  the  states  of  the  system;  z  is  the 


x  =  Fx  +  Fw 
z  =  Hx  +  v 


(1-a) 

(1-b) 


measurement  vector;  F  is  the  system  matrix;  r  is  the  driving 
noise  coefficient  matrix;  H  describes  the  relationship  between 
the  states  and  the  measurement;  w  and  v  are  independent,  zero- 
mean  white  gaussian  noise  process  with  covariance  matrices  Q 
and  R  respectively,  that  in  mathematical  terms  are  given  by: 


E(w(t)w  (t))  -  Q(t) 6  (t-x) ,  E (w(t) )  =  0 
E(v(t) vT(x) )  -  R(t)5(t-T),  E(v(t) )  -  0 


(2-a) 

(2-b) 


2. 


Continuous  Time  Kalman  Filter 


A  continuous  time  Kalman  Filter  is  described  by 

A 

equation  (3),  where  x  is  the  state  estimate;  K  is  a  matrix  of 
constant  filter  gains.  The  implementation  of  the  System 
Model  and  the  Kalman  Filter  is  shown  in  block  diagram  in 
Figure  1. 


x  -  Fx  +  K(z-Hx)  (3) 


MATHEMATICAL  MODEL 

- - 

SYSTEM  MEASUREMENT  I 


KALMAN  FILTER 


I 


I 

I 


Figure  1 

System  Model  and  Kalman  Filter 


The  estimate  error  is  defined  by  equation  (4)  as, 


~  A  ^ 

x  =  x  -  x  (4) 

and  the  differential  equation  for  x  is  given  by 

x  =  (F-KH)x  -  Tw  +  Kv  (5) 


The  differential  equations  for  the  state  of  a  linear  system 
driven  by  white  noise  can  be  expressed  as 


-  •  ■ 

—  |  — 

r  _ 

— 

X 

F-KHl  0 

X 

Kv-  Tw 

— . 

- J-  — 

— 

4- 

- —  — 

• 

_x 

fcu’ 

O 

_ 1 

fw 

(6) 


The  covariance  of  the  es timate- error ,  designated  P,  is  given 
by  equation  (7) .  It  provides  a  statistical  measure  of  the 
uncertainty  in  x. 


P  -  E[xxT] 


(7) 


The  diagonal  elements  of  the  covariance  matrix  are  the  mean 
square  errors  in  the  knowledge  of  the  state  variables.  Also, 
the  trace  of  P  is  the  mean  square  length  of  the  vector  x. 

The  off  diagonal  terms  of  P  are  indicators  of  cross-correlation 
between  the  elements  of  x.  The  covariance  matrix  P  is  obtained 
by  solving  the  linear  Lyapunov  equation  given  by 


(8) 


P  =  (F-KH)P  +  P(F-KH)T  +  TQrT  +  KRKT 
The  eigenvalues  of  the  filter  are  the  roots  of  equation  (9) . 

| SI  -  F  +  KH|  -  0  (9) 

B.  STATE  AUGMENTATION  AND  SHAPING  FILTERS 

When  the  system  random  disturbances  are  correlated  in 
time,  i.e.,  colored  noise,  it  is  necessary  to  use  their 
power  spectral  density  data  in  order  to  develop  a  mathematical 
model  that  produces  an  output  which  duplicates  the  noise 
characteristics  CRef.  2].  Correlated  random  noises  are  taken 
to  be  state  variables  of  a  fictitious  linear  time  invariant 
system  (generally  called  a  shaping  filter)  which  is  itself 
excited  by  white  gauss ian  noise.  Such  a  model  is  given  by 
equation  (10),  where  the  subscript  f,  denotes  filter  and  n, 
is  a  nonwhite,  i.e.,  time-correlated,  gaussian  noise.  The 
filter  output  is  used  to  drive  the  system  as  show  in  Fig.  2. 

• 

Xf  =  Ffxf  4-  rfw  (10-a) 

n  =  Hfxf  (10-b) 

The  dimension  of  the  state  vector  (1)  is  increased  by 
including  the  disturbances  as  well  as  a  description  of  the 
system  dynamics  behavior  in  appropriate  rows  of  an  enlarged 
F  matrix,  this  is  called  state  vector  augmentation. 
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1 _ 


Figure  2 

Shaping  filter  generating  driving  noise. 
Conformation  of  the  Augmented  System. 


The  augmented  state  equation  is  given  by 


(ID 


The  associated  measurement  equation  given  by  equation  (12) 


z 


+  V 


(12) 


C.  SENSITIVITY  TO  PARAMETER  VARIATION 

Observing  the  structure  of  the  Kalman  Filter  illustrated 
in  Figure  1;  the  filter  contains  an  exact  model  of  the  system 
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dynamics.  The  filter  gain  matrix  is  calculated  using  the 
parameters  of  the  exact  model  of  the  dynamics. 

1 .  Incorrect  Implementation  of  Dynamics 

« 

The  analysis  of  how  the  error  covariance  behaves  when 
the  gain  matrix  is  computed  using  wrong  values  of  F  matrix, 
i.e.,  varying  parameters  due  to  different  flight  conditions, 
is  quite  well  explained  in  Reference  ClD.  Figure  3  shows  the 
block  diagram  for  the  system  model  and  the  Kalman  Filter, 
where  the  quantities  K*  and  F*  represent  the  gain  matrix  and 
the  system  dynamics  implemented  in  the  filter. 


< 


Figure  3 

Block  diagram  of  the  System  Model  and  Kalman 
Filter  with  incorrect  implementation  of  dynamics. 


The  equation  for  the  estimate  is  given  by 


x  -  F*x  +  K*(z-Hx)  (13) 

The  error  in  the  estimate  given  by  (14) 

x  =  (F*  -  K*H)x  +  AFx  -  Tw  +  K*v  (14) 

where 


A 

AF  =■  F*  -  F 

The  differential  equations  for  the  states  of  a  linear  system 
driven  by  white  gaussian  noise  given  previously  by  equation 
(6)  becomes  now 


X 

F*  -  K*H  1  AF 

X 

k*v  -  rw 

_x_ 

xt 

—  U 

0  I  F 

X 

+ 

rw 

_ 

(15) 


A 

Letting  x'  be  the  augmented  state  vector,  x'  = 
where  the  covariance  matrix  of  x1  is  given  by: 


E(x'x,T) 


P^|  VT 
V  J  u 


(16) 


where 
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P  A  E(xxT) ,  V  =  E (xxT) ,  U  *  E(xxT) 


The  quantity  of  interest  is  P,  the  covariance  of  x. 

The  error  sensitivity  equations  are  given  by 


P  -  (F*  -  K*H)P  +  P(F*  -  K*H) 1  +  AFV 

+  vtaft  +  rorT  +  k*rk*t 


(17-a) 


T  ,  „.„T 


V  -  FV  +  V(F*  -  K*H)  +  UAF1  -  rQT 


u  *  fu  +  uft  +  rQrT 


( 1 7 -b ) 

(17-c) 


with  initial  conditions 


P(0)  *  -V(0)  =  U(0)  *  E(x(0)x(0) i) 


when  the  actual  system  dynamics  are  faithfully  reproduced  in 
the  filter,  i.e.,  F  *  F*,  AF  =  0,  the  equation  (17)  reduces 
to  the  linear  Lyapunov  equation  given  by  equation  (8) . 

D.  MODAL  COORDINATES  TRANSFORMATION 

The  representation  of  the  system  given  by  equation  (1)  is 
not  unique.  Considering  an  alternative  linear  transformation 
of  the  states  given  in  references  [3]  and  [4]  ,  let  x  =  T£, 
where  C  represents  the  transformation  of  the  states  and  T  is 
the  transformation  matrix  with  the  columns  formed  by  the 
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eigenvectors  of  the  system  matrix  F  (for  a  complex  eigenvalue, 
the  first  column  is  the  real  part  and  the  second  column  is 
the  imaginary  part  of  the  eigenvector) .  The  similarity 
transformation  of  equation  (1)  is  given  by 


5  -  +  Bw 


(18-a) 


z  *  C£  +  v 


(lS-b) 


where 


A  =*  T~ 1  FT ,  B  ■  T~  1  F ,  C  -  HT 

A  case  of  particular  interest  (canonical  form)  occurs 
when  the  resulting  A  matrix  is  diagonal  (the  eigenvalues  of 
the  F  matrix  form  the  diagonal).  The  design  of  the  estimator 
in  modal  coordinates  is  analogous  to  using  transfer  functions 
in  partial  fraction  form.  The  canonical  form  is  more  inform¬ 
ative  than  the  transfer  frunction  method,  since  observability 
and  controllability  can  be  obtained  by  inspection. 

E.  SOLUTION  OF  THE  KALMAN  FILTER  WITH  A  PRESCRIBED  DEGREE 

OF  STABILITY 

The  Kalman  Filter  designed  with  constant  gain  (SKF)  and 
used  as  an  observer  will  diverge  if  there  are  undisturbed, 
neutrally  stable  (UNS)  modes  in  the  system  model.  In  refer¬ 
ences  [40  and  [5]  the  authors  presented  the  idea  of 
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destabilization  of  the  system  model  (1),  the  amount  of  de- 
stabilization  can  be  varied  until  the  resulting  suboptimal 
observer  has  a  specified  degree  of  stability.  The  method 
presented  in  Ref.  [4]  refers  only  to  destabilizing  the  UNS 
modes  in  the  system  model,  that  method  is  called  "modal 
destabilization"  (MDS) .  The  gains  of  the  filter  are  con¬ 
strained  so  that 

Re(Si)  -  -a,  i  =  1,2, ....n  (19) 

where  Re  (Si)  indicates  "real  part  of  (Si),"  Sj . Sn  are 

the  eigenvalues  of  the  filter,  i.e.,  the  roots  of  equation 
(9)  and  a  is  a  specified  positive  number. 

The  original  system  is  destabilized  in  accordance  with 
equation  (20),  where  F'  is  the  resultant  destabilized  matrix, 
E  the  destabilization  matrix  (diagonal)  and  T  is  the  modal 
transformation  matrix  (eigenvector  matrix).  The  matrix  F'  is 
used  in  order  to  calculate  the  modified  gains  of  the  filter 
(suboptimal  gains) . 

F'  -  F  +  TET"1  (20) 


This  method  avoids  the  divergence  of  the  steady-state 


i light  decrease  in  estimation  accuracy. 


1 
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III.  DYNAMIC  AND  MEASUREMENT  SYSTEM  MODELS 


A,  REFERENCE  AXIS  SYSTEM 

The  Reference  Axis  System  in  a  missile  is  centered  on  the 

c.g.  and  fixed  on  the  body  as  follows: 

X  axis,  called  the  roll  axis,  forwards,  along 
the  axis  of  symmetry. 

Y  axis,  called  the  pitch  axis,  outwards  and  to 
the  right  if  viewing  the  missile  from  behind. 

Z  axis,  called  the  yaw  axis,  downwards  in  the 
plane  of  symmetry  to  form  a  right-handed 
orthogonal  system  with  the  other  two. 

The  symbols  from  Appendix  A  define  the  forces  and  moments 
acting  on  the  missile,  the  linear  and  angular  velocities,  and 
the  moments  of  inertia;  those  quantities  are  shown  in  Figure 


Relative  Wind 


Figure  4 

Reference  Axis  System 
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B.  MISSILE  EQUATIONS  OF  MOTION 


The  equations  of  motion  used  in  the  present  study  in  order 
to  represent  the  missile  dynamics  are  quite  well  defined  in 
Reference  [7].  A  linear  dynamical  model  of  the  missile  based 
on  the  rigid  body  approximation  is  appropriate. 

1.  Longitudinal  Motion 

The  longitudinal  motions  of  a  missile  are  modeled  by 
a  fifth-order  system  given  by  equation  (21),  where  the  state 
variables  are  u,  velocity  along  the  X  axis;  w,  velocity  along 
the  Z  axis;  q,  pitch  rate,  6,  pitch  angle  and  h,  altitude. 


Xw 

Zw 

Mw+MwZw 

0 

-1 


(21) 


2 .  Lateral  Motion 

The  lateral  motions  of  a  missile  are  modeled  by  a 
fifth-order  system  given  by  equation  (22)  ,  where  the  state 
variables  defined  are;  6  sideslip  angle;  r  yaw  rate;  p  roll 
rate;  4>  roll  angle;  ip  heading  angle. 


r.~ 

B 

Yv 

-1 

0 

g/v 

0 

B 

r 

Nl 

r 

NP 

0 

0 

r 

P 

= 

4 

LP 

0 

0 

P 

* 

0 

0 

1 

0 

0 

<f> 

L*j 

0 

1 

0 

0 

0 

❖ 

(22) 


C .  MODEL 

The  aerodynamic  data  used  in  the  present  study  is  given 
in  Appendix  B  [Ref  91.  Essentially  the  models  and  noise 
dynamics  are  those  of  reference  [43. 

1 .  Longitudinal  Motion  Estimation 

The  main  disturbance  inputs  are  the  two  wind  velo¬ 
cities  u  and  w  .  Under  the  particular  flight  conditions, 

o  o 

the  turbulance  represented  by  the  fluctuating  parts  of  Ug  and 
Wg  are  color  noise.  They  are  modeled  by  first-order  shaping 
filters  with  white  gaussian  noise  inputs  as  given  in  equation 
(10).  The  resultant  linear  model  is  given  by  equation  (23) 
[Ref  83. 


r-  — 

— 

|—  - 

— 

r~  * 

“g 

-0.413 

0 

ug 

0.413 

0 

Uu 

= 

+ 

0 

-0.853 

"g- 

0 

0.853 

.V 

— 

— 

- 

— J 

(23) 

The  numerical  data  for  the  longitudinal  dimensional 
derivatives  was  used  in  equation  (21) ,  the  resultant  model  is 
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given  by  equation  (24) ,  that  corresponds  to  the  state  vector 
augmentation  given  by  equation  (11).  Units  are  scaled  with 


u,  w,  u  and  w  in  units  of  10  ft/s,  q  in  units  of  0.01  rad/s. 

O  © 


• 

u 

-0.015 

0.004  0 

-0.0322 

0 

-0.015 

0.004 

u 

0 

0 

• 

w 

-0.074 

-0.806  0.824 

0 

0 

-0.074 

-0.806 

w 

0 

0 

• 

q 

-0.749 

-10.7  -1.344 

0 

0 

0.749 

-10.7 

q 

0 

0 

0 

0 

0  1 

0 

0 

0 

0 

0 

+ 

0 

0 

h 

0 

-0.1  0 

0.0824 

0 

0 

0 

h 

0 

0 

ag 

0 

0  0 

0 

0 

-0.413 

0 

u 

g 

0.413 

0 

• 

w 

l\ 

0 

0  0 

0 

0 

0 

-0.853 

w 

LsJ 

0 

0. 

853 

(24; 

0 

in 

units 

of  0.01  rad. 

,  and  h 

in 

units 

of  100 

ft. 

L^wJ 


The  measurement  model  given  by  equation  (25)  assumes 


a  rate  gyro  in  order  to  measure  z^  and  a  barometric  altimeter 


in  order  to  measure  z 


h* 


“z 

o  o  i  o  o  o  6 

q 

’l  0 

v_ 

q 

0 

q 

+ 

zh 

0  0  0  0  1  0  0 

h 

0  1 

vh 

n 

w, 


L"gJ 


(25) 
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2 .  Lateral  Motion  Estimation 

The  main  disturbance  input  is  lateral  wind  Vg.  The 
turbulence  represented  by  the  fluctuating  part  of  Vg  is  color 
noise,  it  is  modeled  as  a  first-order  shaping  filter  with 
white  gaussian  noise  input  as  given  in  equation  (10) .  The 
resulting  shaping  filter  is  as  given  by  equation  (26)  and 
taken  from  Reference  [41. 

3g  -  -0.853Sg  +  0 . 853y 

where  (26) 

6g  *  Vv 

The  numerical  data  for  the  lateral  dimensional  derivatives 
was  used  in  equation  (22)  to  obtain  the  equation  (27).  This 
corresponds  to  the  state  vector  augmentation  given  by 
equation  (11) . 


(27) 
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The  measurement  model  given  by  equation  (28),  represents  the 
situation  where  the  measurement  Zp  is  taking  with  a  roll-rate 
gyro  and  the  measurement  using  a  magnetic  compass. 

3 

r 
P 

8 
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IV.  ANALYSIS 


A.  THE  SIMULATION 

A  computer  program  was  developed  in  order  to  solve  the 
sensitivity  equations  given  by  equation  (17)  .  Basically,  the 
program  was  used  to  handle  a  set  of  105  linear  differential 
equations  in  the  longitudinal  case  and  78  in  the  lateral. 

The  principle  program  output  is  the  time  history  of  the  P 
matrix  and  the  square  root  of  its  diagonal  elements  (the  rms 
estimate  errors). 

The  Kalman  Filter  computer  program  was  the  primary  tool 
used  in  computing  the  gains.  The  program  was  developed  at 
the  Massachusetts  Institute  of  Technology  [Ref.  10]  and 
adapted  for  use  at  the  Naval  Postgraduate  School.  A  second 
computer  program  composed  at  Stanford  University,  OPTSYS  4 
Computer  Program  CRef.  11],  which  utilizes  several  options  of 
the  Kalman  Filter,  was  also  implemented  in  this  analysis.  In 
particular,  the  suboptimal  filter  using  the  modal  destabili¬ 
zation  (MDS)  design  option  was  required  for  the  lateral  case. 

B.  THE  RESULTS 

As  the  problem  is  described,  the  results  are  presented  in 
two  parts,  the  longitudinal  and  the  lateral  case. 

Initially  both  models  given  by  equations  (24)  and  (27) 
were  modified  in  order  to  include  another  state  in  each  of 
them,  the  perturbed  position.  The  resultant  observers  were 
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tested,  but  in  both  cases  they  were  unstable. 

1 .  Longitudinal  Motion  Estimation  Analysis 

A  steady-state  Kalman  Filter  was  designed  [Ref.  43 
and  used  as  a  constant  gain  state  estimator  (equation  (3)). 

The  first  step  in  this  analysis  was  to  reproduce  the 
results  for  the  case  of  exact  implementation  of  dynamics  in 
the  calculation  of  the  Kalman  gain  matrix.  That  analysis  was 
done  using  the  M.I.T.  Kalman  Filter  Computer  Program  [Ref.  10 
The  results  are  shown  below: 


Filter  gain  matrix  K 

0.059 

0.060 

0.264 

-0.161 

3.517 

0.040 

0.001 

-0.080 

0.013 

0.136 

-0.011 

0.035 

-1.288 

0.128 

Filter  Eigenvalues 
-0.310  +  jO .411 
-0.429 
-0.178 
-0.261 

-0.063  +  jO .0743 


rms  estimate  errors 
u  -  2.090  ft/s 
w  -  5.102  ft/s 
q  «  0.416  deg/s 


0.317  deg 
8.2*5  ft 
4.776  ft/s 
5.701  ft/s 
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These  results  are  essentially  identical  to  those  of  Ref.  [4]. 

The  next  objective  was  to  consider  the  incorrect 
implementation  of  dynamics  in  the  Kalman  Filter,  in  order  to 
study  the  sensitivity  of  rms  estimate  errors  to  inaccuracies 
in  the  stability  derivatives.  The  numerical  variation  of  the 
parameters  were  considered  individually  in  order  to  establish 
the  F*  matrix  to  be  used  in  equation  (17)  and  also  for  finding 
the  filter  gain  matrix  K*  to  be  implemented  in  the  same 
equation.  For  each  variation  of  the  parameter  under  obser¬ 
vation  it  was  necessary  to  run  the  M.I.T.  Computer  Program 
[Ref.  10]  in  order  to  find  the  matrix  of  gains  K*  and  then  to 
run  the  sensitivity  covariance  program  in  order  to  study  the 
propagation  of  the  estimate  errors. 

The  results  are  shown  in  Tables  1-8  and  Figures  5-40. 
In  the  Tables,  the  true  value  of  each  parameter  is  marked  with 
an  asterisk.  The  description  of  the  results  are: 

Xu.  The  dimensional  variation  of  X  force  with  forward 
speed  (u)  has  a  nominal  value  of  -0.015.  This  quantity  was 
changed  in  a  range  of  +207o.  The  behavior  of  the  rms  estimate 
errors  are  presented  in  Table  1.  From  that  it  can  be  seen 
that  any  numerical  variation  of  Xu  derivative  does  not  cause 
significant  changes  in  the  nominal  values  of  the  rms  estimate 
errors  of  the  states,  w,  q,  0,  ug  and  wg.  For  the  states 
u  and  h  the  results  are  shown  in  Figures  5-6  respectively, 
with  some  effect,  but  not  significant  for  positive  variation 


Xw.  The  dime*  ional  variation  of  X  force  with  down¬ 
ward  speed  (w)  has  a  nominal  value  of  0.004.  As  before,  the 
numerical  variation  was  performed  in  a  range  of  +207*.  The 
behavior  of  the  rms  estimate  errors  were  tabulated  in  Table 
2.  Checking  the  results  there  is  no  variation  in  the  nominal 
values  of  rms  estimate  errors  of  the  states,  w,  q,  9,  Ug  and 
Wg,  for  any  of  the  changes  made  in  Xw  derivative.  The  vari¬ 
ations  in  the  estimate  errors  for  u  and  h  are  indicated  in 
Figures  7  and  8  respectively.  Increasing  the  value  of  Xw  the 
u  error  decreases  but  h  increases,  while  taking  smaller  values 
of  the  parameter,  the  effect  is  contrary.  Those  changes  in 
the  rms  errors  are  considered  not  important. 

Zu.  The  dimensional  variation  of  Z  force  caused  by 
a  change  in  forward  speed  (u)  has  a  nominal  value  of  -0.074. 
The  effects  of  changing  its  design  value  was  observed  in  a 
range  of  207o  and  its  result  condensed  in  Table  3  as  well  as 
in  Figures  9-14.  All  the  rms  errors  show  some  degree  of 
sensitivity  except  q.  The  most  important  changes  occur  in 
u,  9,  and  h  errors  and  the  large  variation  in  u  can  begin  to 
be  important  in  terms  of  accuracy  in  radial  position. 

Zw.  The  dimensional  variation  of  Z  force  with  down¬ 
ward  speed  (w)  has  a  nominal  value  of  -0.806.  The  results 
for  this  case  are  presented  in  Table  4  and  Figures  13-20. 
Again,  the  variation  in  the  value  of  Zw  was  made  in  a  range 
of  +207o.  .  All  the  rms  estimate  errors  show  a  large  degree  of 


sensitivity  and  it  can  be  considered  that  any  numerical  vari¬ 
ation  of  Zw  beyond  +  2%  is  critical  and  unacceptable. 

Mu.  The  dimensional  variation  of  M  moment  caused  by 
a  change  in  forward  speed  (u)  has  a  nominal  value  of  -0.000786. 
The  value  of  Mu  was  changed  in  a  range  of  207*  and  the  results 
are  given  in  Table  5  as  well  as  Figures  21-26.  Any  variation 
in  Mu  quantity  does  not  cause  important  changes  in  the  value 
of  the  rms  estimate  errors  of  the  states,  a,  Ug  and  Wg.  For 
For  the  states  u,  w,  9,  and  ft,  the  variation  in  the  errors 
can  be  considered  with  some  significant  effect  for  large 
deviations  of  Mu  value,  i.e.,  more  than  +10%. 

Mw.  The  dimensional  variation  of  M  moment  with  speed 
(w)  has  a  nominal  value  of  -0.0111.  Its  numerical  variation 
was  taken  in  a  range  of  +107o.  From  Table  6  and  Figures  27-32, 
it  can  be  seen  that  any  alteration  in  the  true  value  of  Mw  is 
reflected  with  a  strong  effect  on  all  the  rms  estimate  errors. 
This  derivative  can  be  considered  the  most  critical  in  the 
longitudinal  motion  estimation  case. 

Mq .  The  dimensional  variation  of  pitching  moment  with 
pitch  rate  (q)  has  a  nominal  value  of  -0.924.  The  results 
are  given  in  Table  7  and  Figures  33  and  34  for  a  range  of 
+20%  in  the  numerical  variation  of  Mq.  The  sensitivity  of  the 
rms  estimate  errors  due  to  changes  in  this  parameter  is  min¬ 
imum  for  all  the  states. 
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Mw.  The  dimensional  variation  of  pitching  moment 
with  rate  of  change  of  downward  speed  (w)  has  a  nominal  value 
of  -0.00051.  Table  8  and  Figures  35-40  presents  the  infor¬ 
mation  about  the  changes  of  the  rms  estimate  errors  in  a 
range  of  +20%  in  the  variation  of  Mw  derivative.  All  the 
errors  show  some  degree  of  sensitivity  and  with  more  than  +2% 
in  the  alteration  of  the  Mw  value,  the  variation  in  the  errors 
begins  to  be  significant. 

2 .  Lateral  Motion  Estimation  Analysis 

The  dynamic  system  (equation  (27))  and  measurement 
models  (equation  (28))  were  transformed  into  modal  coordin¬ 
ates  in  accordance  with  equation  (18).  This  was  done  using 
the  OPTSYS  4  Computer  Program  [Ref.  11];  the  results  are 
given  below: 


(29) 
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+ 
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(30) 


These  results  are  identical  to  those  given  in  Ref. 

L43.  In  the  same  reference  the  author  gives  a  complete 
analysis  of  controllability  and  observability  using  the  re¬ 
sults  given  in  equations  (29)  and  (30)  .  From  these  can  be 
seen  that  the  spiral  mode  £53  has  its  eigenvalue  close  to  the 
origin  and  it  is  almost  undisturbed  by  the  process  noise;  also 
it  is  unobservable  with  the  measurement  z0.  Using  a  steady- 
state  Kalman  Filter  as  an  observer  fail  to  estimate  the 
heading  mode  £H  and  its  estimation  of  the  spiral  mode  £S  is 
too  slow.  The  next  objective  was  to  take  the  SKF  and  to  con¬ 
sider  the  situation  of  parameter  variations  in  the  dynamic 
implemented  in  the  filter.  With  the  true  values  of  the  F 
matrix  implemented  in  the  filter  the  results  are  shown  below: 


Filter 

Gain  Matrix  K 

0.051 

-0.967 

-1.536 

0.411 

2.695 

-0.004 

0.386 

-0.789 

-0.005 

0.906 

-1.713 

0.655 
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Filter  Eigenvalues 

-2.350  +  j  2 . 594 
-0.624  +  jO.492 
-0.00125 
0.0 


rms  Estimate  Errors 

VB  =■  3.329  ft/s 
r  =  0.244  deg/ s 
p  =  0.377  deg/s 
$  =  0.222  deg 
$  ■  0.214  deg 
VBg  -  5.506  ft/s 

These  results  are  essentially  identical  to  those 
from  Ref.  [4],  The  filter  was  found  to  be  stable  but  with  a 
narrow  margin  of  stability.  Any  attempt  to  change  parameters 
resulted  in  a  failure  of  the  estimator  due  to  divergence  with 
the  most  minimum  variation  (i.e.,  +1%)  of  any  of  the  para¬ 
meters.  This  result  complements  the  analysis  given  previously 
[Ref.  4]  and  it  shows  definitely  that  it  is  impossible  to  use 
the  SKF  as  an  estimator  for  the  lateral  motion  with  the  actual 
configuration  of  the  model  given  by  equations  (27)  and  (28). 

The  alternative  solution  was  to  design  a  suboptimal 
observer  (MDS)  with  the  constrain  (19)  as  given  in  Ref.  C 471 , 
a  -  0.029  and  using  equation  (20)  in  the  MDS  option  the 
computer  program  OPTSYS  4  [Ref.  117 .  The  results  are: 
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Filter 

Gain  Matrix  K 

-0.073 

-0.871 

-1.560 

0.430 

2.731 

-0.033 

0.339 

-0.753 

-0.033 

0.928 

-1.622 

0.585 

Filter 

Eigenvalues 

-2.350  +  j  2 . 589 
-0.624  +  j0.492 
-0.0291  +  j0.005 


rms  Estimate  Errors 


V0 

■ 

3.799 

ft/ s 

r 

a 

0.245 

deg/s 

P 

a 

0.379 

deg/s 

a 

0.226 

deg 

£ 

- 

0.216 

deg 

vgg 

- 

5.712 

ft/s 

These  results  are  essentially  identical  to  those 
from  Ref.  C^H. 

The  sensitivity  of  the  lateral  rms  estimate  errors 
due  to  inaccuracies  in  the  stability  derivatives  was  studied 
using  the  suboptimal  filter  (MDS)  design  and  following  the 
same  procedure  explained  before  for  the  longitudinal  case. 
The  results  are  shown  in  Tables  9-15  and  Figures  41-76.  The 
results  are  described  in  detail  as  follows: 
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Yv.  The  dimensional  variation  of  Y  force  with  side 
velocity  (v)  has  a  nominal  value  of  -.0868.  This  value  was 
changed  in  a  range  of  4-207,  and  the  results  appear  in  Table  9 
and  Figures  41-44.  r,  p,  ip,  and  V6g  show  almost  no  sensi¬ 
tivity  to  the  variation  in  Yv  parameter,  VS  changes  but  it 
can  be  considered  of  secondary  importance.  The  most  sensi¬ 
tivity  change  occurs  in  <p  and  it  begins  to  be  important  for  a 
variation  of  +57,  in  Yv  value. 

Ng.  The  dimensional  variation  of  yawing  moment  about 
Z  axis  with  sideslip  angle  (S)  has  a  nominal  value  of  2.14. 
Table  10  and  Figures  45-50  give  the  results  for  a  variation 
of  N'  derivative  in  a  range  of  4-107,.  All  the  rms  estimate 

P  * 

errors  except  tp  present  strong  sensitivity,  especially  VB 
and  <f>.  With  a  variation  of  more  than  +17,  the  estimation  of 
these  errors  begin  to  show  large  values  and  the  filter  takes 
a  long  time  to  reach  the  steady  state  value  (more  than  150  s) . 
For  that  reason,  any  variation  in  the  true  value  of  Ng  with 
the  purpose  of  implementation  in  a  Kalman  Filter  as  an  esti¬ 
mator  must  be  considered  unacceptable. 

N^.  The  dimensional  variation  of  yawing  moment  about 
Z  axis  with  yaw  rate  (r)  has  a  nominal  value  of  -0.228.  In 
a  range  of  +207,  the  variation  of  N^  value  was  performed  and 
the  results  tabulated  in  Table  11  as  well  as  the  important 
variations  registered  in  Figures  51-54.  The  effects  on 
sensitivity  of  all  the  rms  estimate  errors  are  secondary  and 
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there  is  no  important  alteration  in  their  values  due  to  large 
variations  in  the  parameter. 

N£.  The  dimensional  variation  of  yawing  moment  about 
Z  axis  with  roll  rate  (p)  has  a  nominal  value  of  -0.0204. 

This  number  was  changed  in  a  range  of  +20%  and  the  results 
observed  are  given  in  Table  12  and  Figures  55-60.  r,  p,  i ), 
and  VSg  rms  errors  do  not  present  any  important  variation. 

The  sensitivity  in  rms  estimate  error  is  observed  in  Vg  and 
5,  in  particular.  For  the  last  one,  the  change  is  too  large 
for  a  variation  of  5%  in  the  value  of  the  parameter. 

L& .  The  dimensional  variation  of  rolling  moment 
about  X  axis  with  sideslip  angle  (g)  has  a  nominal  value  of 
-4.41.  Its  variation  was  given  just  in  a  margin  of  +10%  be¬ 
cause  with  larger  changes  the  estimator  starts  to  diverge. 

The  results  obtained  are  given  in  Table  13  and  Figures  60-66. 
All  the  rms  estimate  errors  show  some  degree  of  sensitivity 
due  to  the  variation  in  Lg  parameter  and  with  1%  of  change 
some  of  the  errors  like  Vg”  and  b  present  very  large  vari¬ 
ations.  For  that  reason  this  derivative  must  be  considered 
critical  in  the  implementation  of  the  Kalman  Filter  as  an 
estimator. 

Lr.  The  dimensional  variation  of  rolling  moment 
about  X  axis  with  yaw  rate  (r)  has  a  nominal  value  of  -0.334. 
The  results  presented  in  Table  14  and  Figures  67-72  are  the 
product  of  the  observation  for  the  variation  of  the  parameter 
L-£  in  a  range  of  +10%  and  it  can  be  considered  as  the  most 
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important  stability  derivative  for  the  lateral  model,  because 
the  dynamics  implemented  in  the  filter  does  not  allow  any 
variation  of  its  true  value,  otherwise,  the  estimation  of  the 
errors  becomes  to  be  too  large  and  probably  the  filter  can 
diverge,  ^n  other  words,  the  sensitivity  of  the  estimator 
to  any  variation  in  L^.  value  is  extremely  critical. 

L£.  The  dimensional  variation  of  rolling  moment 
about  X  axis  with  roll  rate  has  a  nominal  value  of  -1.204. 

The  behavior  of  the  rms  estimate  errors  were  observed  taking 
a  variation  of  +10%  of  the  L$  parameter  and  they  are  given 
in  Table  15  and  Figures  73-76.  Vg  and  r  are  the  variables 
that  present  some  degree  of  sensitivity  in  the  increment  of 
their  rms  estimate  errors,  respectively,  and  they  begin  to  be 
important  with  a  variation  of  more  than  +5%  of  the  L£  true 
value . 
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U  RMS  ESTIMATE  ERROR  VS  XU 


Figure  5 
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Sensitivity  of  u  and  h  rms  estimate  errors  vs 
variation  in  Xu  stability  derivative 

H  RMS  ESTIMATE  ERROR  VS  XU 


Figure  6 
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U  RMS  ESTIMRTE  ERROR  VS  XW 

Figure  7 


Figures  7  &  8.  Sinsitivity  of  u  and  h  rms  estimate  errors  vs 
variation  in  Xw  stability  derivative. 
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Sensitivity  of  q  and  0  rms  estimate  errors  vs 
variation  in  Mw  stability  derivative. 
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Q  RMS  ESTIMATE  ERROR  VS  MWD0T 


re3  37  &  38.  Sensitivity  of  q  and  9  nns  estimate  errors  vs 
variation  in  M.w  stability  derivative. 
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VB  RMS  ESTIMRTE  ERROR  VS  YV 


Figure  41 
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Sensitivity  of  Vg  and  p  rms  estimate  errors  vs 
variation  in  Yv  stability  derivative. 


P  RMS  ESTIMRTE  ERROR  VS  YV 


Figure  42 
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P  RMS  ESTIMATE  ERROR  VS  NB  ’ 


Figures  47  &  48.  Sensitivity  of  p  and  0  rms  estimate  errors  vs 
variation  of  N*  stability  derivative. 
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Figure  48 


PSI  RMS  ESTIMRTE  ERROR  VS  NB 


Figure  49 
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50.  Sensitivity  of  $  and  Vg„  rms  estimate  errors  vs 
variation  in  N'  stability  derivative. 
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Figure  50 


Figure  52 


PHI  RMS  ESTIMRTE  ERROR  VS  NR 


Figure  53 
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54^.  Sensitivity  of  9  and  Vg  rms  estimate  errors  vs 
variation  in  N*  stability  derivative. 
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Figure  54 


Figure  56 


P  RMS  ESTIMATE  ERROR  VS  NP  ' 

Figure  57 
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Sensitivity  of  p  and  9  rms  estimate  errors  vs 
variation  of  fT  stability  derivative. 
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Figure  58 


Figure  60 


Figure  62 
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VB  RMS  ESTIMATE  ERROR  VS  LR  ' 


Figures  67  &  68.  Sensitivity  of  Vg  and  r  rms  estimate  errors  vs 
variation  of  stability  derivative. 
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Sensitivity  of  p  and  9  rras  estimate  errors  vs 
variation  of  1/  stability  derivative. 
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Figure  70  _ 


Figure  72 
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Figures  73  &  74.  Sensitivity  of  Vg  and  r  rms  estimate  errors  vs 
variation  in  1/  stability  derivative. 
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PHI  RMS  ESTIMATE  ERROR  VS  LP 


Figure  75 
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Sensitivity  of  §  and  VB  rms  estimate  errors  vs 
variation  in  Z/  stability  derivative. 
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Figure  76 


V.  CONCLUSIONS  AND  RECOMMENDATIONS 


A.  CONCLUSIONS 

Using  steady-state  Kalman  Filters  as  observers  in  an 
inertial  navigation  system,  the  sensitivity  of  rms  estimate 
errors  to  inaccuracies  in  the  stability  derivatives  was 
studied.  The  conclusions  reached  at  the  end  of  this  research 
are  presented  in  two  parts  as  they  were  analyzed. 

1 .  Longitudinal  Motion  Estimator 

a.  Numerical  variations  in  the  dimensional  deriv¬ 
atives;  Xu,  Xw  and  Mq  do  not  show  any  important  effect  in  the 
rms  estimate  errors.  The  value  of  these  parameters  as  imple¬ 
mented  in  the  filter  can  have  large  variations  with  respect 
to  their  nominal  values  (perhaps  a  tolerance  of  +20%) . 

b.  Changes  in  the  stability  derivatives;  Zu,  Mu 
and  MiJr  are  reflected  with  variation  in  almost  all  the  rms 
estimate  errors.  In  particular,  large  variations  in  u  rms 
estimate  error  can  begin  to  be  important  in  terms  of  accuracy 
in  the  radial  position.  These  parameters  can  be  implemented 
in  the  dynamics  of  the  filter  with  some  degree  of  tolerance 
with  respect  to  their  true  value  (for  the  case  under  analysis 
no  more  than  +5%) . 

c.  The  most  important  effects  are  found  with  the 
variations  of  the  stability  derivatives;  Zw  and  Mw.  Zw  gives 
the  strongest  alterations  in  all  the  rms  estimate  errors  and 


small  changes  (+27,  the  tolerance  In  the  case  under  study)  in 
this  parameter  are  unacceptable  in  the  filter.  For  Mw  the 
situation  is  quite  similar  and  the  value  of  this  derivative 
is  very  important.  In  general,  Zw  and  Mw  must  be  faithfully 
reproduced  in  the  filter. 

2 .  Lateral  Motion  Estimator 

It  is  not  possible  to  use  the  SKF  as  an  observer 
under  the  particular  configuration  of  the  models  given  for 
lateral  motion.  The  filter  is  stable  but  with  a  narrow  margin 
of  stability.  The  most  minimum  numerical  variation  (i.e., 

+17,)  in  the  value  of  any  stability  derivative  results  in  a 
failure  of  the  filter  due  to  divergence.  This  result  com¬ 
plements  the  discussion  given  in  Ref.  [4].  Using  a  sub- 
optimal  filter  (with  modified  gains),  the  effect  of  the 
numerical  variation  in  each  of  the  stability  derivatives 
are: 

a.  Numerical  variations  in  the  dimensional  deriva¬ 
tives;  Yv,  N^,  and  Lp  do  not  cause  important  effects  in  any  of 
the  rms  estimate  errors.  The  reproduction  of  these  para¬ 
meters  in  the  filter  are  not  required  to  be  too  accurate 
(considering  in  this  case  a  tolerance  of  +107,)  . 

b.  Np  is  a  parameter  that  only  accepts  small  vari¬ 
ations  in  its  value  (in  the  problem  discussed,  27,  of  toler¬ 
ance)  because  it  has  strong  effects  in  the  estimation  of  $ 
rms  estimate  error. 
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c.  The  stability  derivative  Ng  is  one  of  the  most 
important  in  terms  of  faithful  reproduction  in  the  dynamics 
of  the  filter.  In  general,  it  can  be  said  that  any  numerical 
variation  in  its  true  value  is  not  allowed. 

d.  Finally,  Lg  and  present  the  most  dramatic 
situation,  because  a  minimum  change  in  its  value  is  reflected 
with  very  large  changes  in  the  estimation  of  rms  errors  and 

it  can  result  in  divergence  of  the  Kalman  Filter.  The  lateral 
motion  estimator  design  requires  that  these  stability  deriva¬ 
tives  must  be  reproduced  in  the  filter  with  the  maximum  of 
accuracy,  otherwise,  the  result  can  be  failure  of  the  filter. 

Of  the  two  estimators  used  in  the  implementation  of 
the  INS,  the  lateral  filter  is  more  sensitive  to  variation  in 
its  parameter  value.  It  should  be  remembered  that  the  con¬ 
clusions  reached  in  this  analysis  apply  specifically  to  the 
model  being  analyzed.  Other  models  involving  different 
missiles  may  indicate  different  sensitivities. 

Nevertheless,  it  is  felt  that  the  conclusions  presented 
in  this  thesis  are  of  general  validity. 

B.  SUGGESTIONS  FOR  FURTHER  ANALYSIS 

The  most  obvious  extension  of  the  work  would  be  to  test 
the  estimators  using  the  real  data  for  a  missile. 

Further  study  could  be  to  consider  the  same  problem  de¬ 
signing  the  estimators  with  a  model  that  includes  the  addition 
of  the  estimation  in  position  in  order  to  analyze  the  problem 
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in  terms  of  miss  distance  in  radial  position  for  a  missile 
that  at  the  end  of  the  mid- course  guidance  phase  must  hit  a 
"basket"  (terminal  guidance  phase). 
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VI .  SUMMARY 


The  great  advantage  of  the  analysis  in  the  sensitivity 
of  rms  estimate  errors  to  inaccuracies  in  the  stability 
derivatives  is  that  it  points  out  clearly  which  derivatives 
are  worthy  of  detailed  accurate  determination  and  which  are 
not. 

The  following  table  summarizes  the  level  of  importance  of 
each  of  the  stability  derivatives  in  the  implementation  of 
the  steady-state  Kalman  Filters  to  be  used  as  estimators  in 
an  INS. 
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NI  **  Not  Important 

SI  -  Secondary  (Relative)  Importanc 

PI  -  Primary  Importance 


APPENDIX  A 
LIST  OF  SYMBOLS 


Regular  Symbols 

A 

B 

C 

D 

E 

F 

f 

F' 

g 

H 

H 

h 

INS 

K 

L 

M 

MDS 

N 

n 

P 

P 

Q 

q 

R 

r 

S 

SKF 

T 


Definition 

Modal  transformation  of  F  matrix 
Modal  transofrmation  of  r  matrix 
Modal  transformation  of  H  matrix 
Dutch  roll  mode 
Destabilization  matrix 
System  dynamics  matrix 
Subscript  for  filter 
Destabilized  matrix 
Subscript  for  wind  speed 
Measurement  matrix 
Heading  Mode 
Altitude 

Inertial  Navigation  System 

Kalman  Filter  gain  matrix 

Rolling  moment  (about  X  axis) 

Pitching  moment  (about  Y  axis) 

Modal  destabilization 

Yawing  moment  (about  Z  axis) 

Non-white  gaussian  noise 

Covariance  propagation  of  the  estimate 
error  matrix 

Perturbed  roll  rate 

Covariance  matrix  of  w 

Perturbed  pitch  rate 

Covariance  matrix  of  v 

Perturbed  yaw  rate 

Spiral  mode 

Steady-state  Kalman  Filter 
Transformation  matrix 
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Regular  Symbols 


Definition 


UNS 

u 

V 

V 
w 
w 
X 

X 

A 

X 

X 

V 
2 
z 


Undisturbed  neutrally  stable 

Perturbed  forward  speed  (along  X  axis) 

Forward  velocity 

Perturbed  side  velocity 

Driving  white  gauss ian  noise 

Perturbed  downward  velocity 

Reference  axis 

State  vector  of  the  system 

State  estimate  vector 

Estimate  error  vector 

Reference  axis 

Measurement  vector 

Reference  axis 


Greek  Symbols 


Definition 


0 

0 

r 

a 

a 

S 


Heading  Angle 

Perturbed  pitch  attitude  angle 
Perturbed  bank  (roll)  angle 
Sideslip  angle 
Driving  noise  matrix 
Eigenvalue  constrain 
Standard  deviation 
Transformed  state  vector 


APPENDIX  B 

AERODYNAMIC  DATA  AND  PROBABILISTIC  INFORMATION 

v  =  820  ft/s 

1 .  Longitudinal  Model 

a.  Dimensional  Derivatives 


Xu 

* 

-0.015 

1/s 

Xw 

3 

0.004 

1/s 

Zu 

3 

-0.074 

1/s 

Zw 

- 

-0.0806 

1/s 

Mu 

- 

-0.0786 

1/s-ft 

Mw 

* 

-0.0111 

1/s-ft 

Mq 

= 

-0.924 

1/s-rad. 

Mw 

- 

-0.00051 

1/ft 

b.  Disturbance  Noise  Standard  Deviation 

au  -  aw  -  1.15  1/s  (10  ft/s)2 

(7  ft/s  rms  gust  with  a  930-ft  correlation 
distance) . 

c.  Observation  Noise  Standard  Deviation 
Oq  *  0.15  s  (0.01  rad/s) 

oh  -  0.05  s  (100  ft)2 

2 .  Lateral  Models 

a.  Dimensional  Derivatives 

Yv  -  -0.0868  1/s 
Ng  -  2.14  1/s2 

Ny  -  -0.228  1/s 

Np  -  -0.0204  1/s 
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Lg  -  -4.41  1/s2 

Lj.  -  0.034  1/s 

-  -1.181  1/s 

b.  Disturbance  Noise  Standard  Deviation 

a  =  1. 63x10"^  1/s  (7  ft/s  rms  gust  with  a 

930  ft  correlation  distance) 

c.  Observation  Noise  Standard  Deviation 
ap  -  1 . 5x10" 5  s 

»  1.5x10"^  1/s 
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COMPUTER  OUTPUT 


Table  1.  rms  estimate  errors  for  longitudinal  motion  estimator 
with  variation  in  Xu  derivative. 


Xu 

u 

ft/s 

w 

ft/s 

q 

deg/s 

5 

deg 

h 

ft 

Ba 

ws 

ft/s 

-0.018 

2.096 

5.102 

0.416 

0.317 

8.248 

4.776 

5.701 

-0.0165 

2.094 

5.102 

0.416 

0.317 

8.246 

4.776 

5.701 

-0.01575 

2.091 

5.102 

0.416 

0.317 

8.240 

4.776 

5.701 

-0.015  * 

2.090 

5.102 

0.416 

0.317 

8.245 

4.776 

5.701 

-0.01425 

2.088 

5.103 

0.416 

0.317 

8.260 

4.775 

5.701 

-0.0135 

2.089 

5.103 

0.416 

0.317 

8.280 

4.775 

5.701 

-0.012 

2.092 

5.103 

0.416 

0.317 

i - 

8.340 

.. 

4.775 

5.701 

Table  2.  rms  estimate  errors  for  longitudinal  motion  estimator 
with  variation  in  Xw  derivative. 


Xw 

U 

ft/s 

w 

ft/s 

q 

deg/s 

5 

deg 

h 

ft 

m 

w 

8 

ft/s 

0.0048 

2.070 

5.102 

0.416 

0.317 

8.319 

4.776 

5.701 

0.0044 

2.080 

5.102 

0.416 

0.317 

8.282 

4.776 

5.701 

0.0042 

2.086 

5.102 

0.416 

0.317 

8.257 

4.776 

5.701 

2.090 

5.102 

0.416 

0.317 

8.245 

4.776 

5.701 

0.0038 

2.100 

5.102 

— 

0.416 

0.317 

8.223 

4.776 

5.701 

0.0036 

2.103 

5.102 

0.416 

0.316 

8.215 

4.776 

5.701 

0.0032 

2.110 

5.101 

0.416 

0.316 

8.170 

4.776 

5.701 

Table  3.  rms  estimate  errors  for  longitudinal  motion  estimator 
with  variation  in  Zu  derivative. 


Zu 

u 

ft/ s 

w 

ft/s 

q 

deg/s 

e 

deg 

h 

ft 

a 

ft?s 

(J 

ft?s 

-0.0888 

1.885 

5.106 

0.416 

0.322 

9.310 

4.776 

5.707 

-0.0814 

1.974 

5.104 

0.416 

0.319 

8.808 

4.775 

5.703 

-0.0777 

2.026 

5.194 

0.416 

0.318 

8.579 

4.775 

5.702 

-0.0740  * 

2.090 

5.102 

0.416 

0.317 

8.245 

4.776 

5.701 

-0.0703 

2.122 

5.100 

0.416 

0.315 

7.800 

4.777 

5.700 

-0.0666 

2.270 

5.095 

0.416 

0.313 

7.101 

4.777 

5.697 

-0.0592 

2.32 

5.094 

0.416 

0.311 

7.000 

4.778 

5.695 

Table  4.  rms  estimate  errors  for  longitudinal  motion  estimator 
with  variation  in  Zw  derivative. 


Zw 

u 

ft/s 

w 

ft/s 

q 

deg/s 

9 

deg 

R 

ft 

u 

ttfa 

w 

ftfs 

-0.9612 

30.08 

6.172 

0.486 

0.440 

17.97 

4.778 

7.138 

-0.8866 

11.80 

5.810 

0.428 

0.415 

33.50 

4.785 

5.947 

-0.8463 

5/345 

5/259 

0.421 

0.400 

22.776 

— 

4.779 

5.737 

-0.806  * 

2.090 

5.102 

0.416 

0.317 

8.245 

4.776 

5.701 

-0.7657 

2.668 

5.032 

0.412 

0.219 

16.903 

4.772 

5.710 

-0.7256 

3.188 

5.035 

0.407 

0.200 

21.026 

4.769 

5.746 

-0.665 

3.260 

5.065 

0.406 

0.185 

23.000 

4.767 

5.794 
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Table  5.  rms  estimate  errors  for  longitudinal  motion  estimator 
with  variation  in  My  derivative. 


Mu 

U 

ft/s 

w 

ft/s 

q 

deg/s 

0 

deg 

H 

ft 

X/s 

*g 

ft/s 

-0.000943 

2.234 

5.061 

0.416 

0.305 

6.230 

4.775 

5.689 

-0.000865 

2.115 

5.089 

0.416 

0.310 

6.640 

4.775 

cm 

-0.000825 

2.104 

5 .094 

0.416 

0.314 

7.531 

4.776 

5.698 

-0.000786* 

2.090 

5.102 

0.416 

0.317 

8.245 

4.776 

5.701 

-0.000747 

1.993 

5.105 

0.416 

0.318 

8.595 

4.777 

5.703 

-0.000707 

1.866 

5.108 

0.416 

0.319 

8.832 

4.779 

5.705 

-0.000629 

1.566 

5.111 

0.416 

0.322 

9.178 

4.783 

5.708 

Table  6.  rms  estimate  errors  for  longitudinal  motion  estimator 
with  variation  in  Mw  derivative. 


Mw 

a 

ft/s 

w 

ft/s 

q 

deg/s 

9 

deg 

h 

ft 

a8 

ft/s 

wg 

ft/s 

-0.01165 

18.43 

8.350 

0.502 

0.455 

29.870 

4.778 

5.695 

-0.0113 

5.163 

5.142 

0.433 

0.373 

17.790 

4.778 

5.694 

-0.0112 

3.110 

5.113 

0.418 

0.321 

10.427 

4.777 

5.699 

-0.0111* 

2.090 

ms 

0.416 

0.317 

8.245 

4.776 

5.701 

-0.0109 

5.206 

— 

5.018 

0.419 

0.325 

16.650 

4.776 

5.700 

-0.01055 

13.652 

5.342 

0.447 

m 

12.204 

4.776 

5.918 

-0.00999 

19.13 

18.95 

0.475 

68.98 

4.756 

6.102 

Table  7.  rms  estimate  errors  for  longitudinal  motion  estimator 


Mq 

-1.109 

-1.016 

-0.970 


with  variation  in  Mq  derivative. 

u  w  q  0 

ft/s  ft/s  deg/s  deg 


r  u„  w 

h  g  g 

ft  ft/s  ft/s 


2.091  5.102  0.416  0.316  8.230  4.776  5.701 

2.091  5.102  0.416  0.316  8.234  4.776  5.701 

2.091  5.102  0.416  0.317  8.236  4.776  5.701 


-0.924* 

2.090 

5.102 

0.416 

0.317 

8.245 

4.776 

5.701 

-0.880 

2.090 

5.102 

0.416 

0.316 

8.244 

4.776 

5.701 

-0.832 

2.089 

5.102 

0.416 

0.316 

8.236 

4.776 

5.701 

-0.7392 

2.089 

5.102 

0.416 

0.316 

8.232 

4.776 

5.701 

Table  8.  rms  estimate  errors  for  longitudinal  motion  estimator 
with  variation  in  M$  derivative. 


Mw 

U 

ft/s 

w 

ft/s 

q 

deg/s 

9 

deg 

h 

ft 

u 

ftfs 

w 

ft?s 

-0.00061 

2.610 

5.053 

0.417 

0.273 

10.665 

4.775 

5.688 

-0.00056 

2.476 

5.089 

0.417 

0.295 

6.301 

4.776 

5.703 

-0.00053 

2.398 

5.091 

0.417 

0.304 

4.150 

4.776 

5.698 

-0.00051* 

2.090 

5.102 

0.416 

0.317 

8.245 

4.776 

5.701 

-0.00049 

2.491 

5.108 

0.416 

0.322 

9.757 

4.776 

5.702 

-0.00046 

2.976 

5.118 

0.415 

0.332 

■ 

12.067 

4.776 

5.703 

-0.00041 

3.570 

5.124 

0.415 

0.340 

13.536 

4.776 

5.703 

Table  9.  rms  estimate  errors  for  lateral  motion  estimator  with 
variation  in  Y  derivate. 


Yv 

vB 

ft/s 

r 

deg/s 

P 

deg/s 

0 

deg 

$ 

deg _ 

A 

-.10416 

3.914 

0.246 

0.380 

0.236 

0.216 

5.743 

-.09548 

3.856 

0.246 

0.379 

0.231 

0.216 

5.728 

-.09110 

3.827 

0.245 

0.379 

0.228 

0.216 

5.719 

-.0868  * 

3.799 

0.245 

0.379 

0.226 

5.719 

-.08246 

3.769 

0.245 

0.378 

0.223 

0.215 

5.702 

-.07812 

3.741 

0.245 

0.378 

0.220 

0.215 

5.694 

-.06944 

3.678 

0.245 

0.377 

0.214 

0.215 

5.677 

Table  10.  rms  estimate  errors  for  lateral  motion  estimator 
with  variation  in  N§  derivative. 


N 1 

D 

v8 
ft/ a 

r 

deg  /s 

P 

_ deg/s 

_ des _ 

5 

deg 

vgg 

ft/s _ 

2.354 

6.889 

0.280 

0.233 

7.719 

2.247 

5.129 

0.266 

0.384 

0.501 

0.230 

6.220 

2.1828 

4.717 

0.253 

0.383 

0.404 

0.223 

6.052 

2.14  * 

3.799 

0.245 

0.379 

0.226 

0.216 

5.712 

2.0972 

4.125 

0.237 

i 

0.371 

0.325 

0.217 

5.938 

2.033 

4.956 

0.224 

0.353 

0.655 

0.218 

6.543 

1.926 

8.781 

0.201 

0.299 

1.101 

0.220 

8.240 

Table  11.  nns  estimate  errors  for  lateral  motion  estimator 
with  variation  in  N'  derivative. 


-0.2736 

3.790 

0.248 

0.380 

0.225 

0.217 

5.704 

-0.2508 

3.791 

0.246 

0.379 

0.225 

0.216 

5.706 

-0.2394 

3.794 

0.246 

0.379 

0.225 

0.216 

5.710 

-0.228  * 

3.799 

0.245 

0.379 

0.226 

— — 

0.216 

5.712 

-0.2166 

3.805 

0.245 

0.379 

0.226 

0.216 

5.714 

-0.2052 

3.815 

0.246 

0.379 

0.228 

0.216 

5.719 

-0.1824 

3.834 

0.248 

0.379 

0.231 

0.216 

5.727 

Table  12.  ras  estimate  errors  for  lateral  motion  estimator 
with  variation  in  Nn  derivative. 


Table  13.  rms  estimate  errors  for  lateral  motion  estimator 
with  variation  in  L 


-4.8510 

7.498 

0.120 

0.338 

0.885 

0.089 

2.266 

-4.6305 

5.621 

0.205 

0.360 

0.539 

0.178 

4,244 

-4.4982 

4.642 

0.240 

0.375 

0.236 

0.211 

5.529 

-4.41  * 

3.799 

0.245 

0.379 

0.226 

0.216 

5.712 

-4.3659 

4.268 

0.249 

0.382 

0.319 

0.219 

5.875 

-4.3218 

4.640 

0.250 

0.390 

0.402 

0.220 

6.295 

-4.1895 

5.246 

0.253 

0.393 

0.492 

0.224 

6.238 

Table  14.  rms  estimate  errors  for  lateral  motion  estimator 
with  variation  in  Li. 


\ 

ft/s 


-0.3407 


-0.334  * 


-0.3273 


-0.3173 


-0.3006 


r 

deg/s 

P 

deg/s 

0.233 

0.352 

0.236 

0.363 

0.240 

0.370 

0.245 

0.379 

0.209 

0.365 

0.206 

0.354 

0.197 

0.350 

7.746 


5.712 


6.613 


7.198 


7.309 
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ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

SENSITIVITY  COVARIANCE  PROGRAM 

THIS  PROGRAM  IS  USED  TO  SOLVE  THE  ERROR  SENSITIVITY 
EQUATIONS  WHEN  THERE  IS  AN  INCORRECT  IMPLEMENTATION 
OF  OYNAMICS  IN  THE  DESIGN  OF  THE  KALMAN  FILTER. THE 
EQUATIONS  BECOME 

PD  OT-(F*-K*H)  P+P  (  F  *-K*H  )  T*DFV+VT  DFT  +  GQGT  +  K#RK*T 
VDOT=FV+V  ( F*-K*H )  T  ♦UDFT-GQGT 
UOOT=FU+UFT +GQGT 

THE  PRINCIPAL  PROGRAM  INPUTS  ARE  THE  FOLLOWING  CO¬ 
LLECTION  OF  SYSTEM  AND  FILTER  MATRICES 

PO  THE  INITIAL  COVARIANCE  MATRIX! NXN ) 

F  THE  TRUTH  MODEL  DYNAMICS  MATRIX! NXN) 

F*  THE  FILTER  MODEL  DYNAMICS  MATRIX(NXN) 

H  THE  TRUTH  MODEL  MEASUREMENT  MATRIX! LXN) , WHERE 
L  IS  THE  MEASUREMENT  VECTOR  DIMENSION 
GQGT  THE  INPUT  NOISE  COVARIANCE  MATRIX(NX) 

R  THE  MEASUREMENT  NOISE  COVARIANCE  MATRIX(LXL) 

K*  FILTER  GAIN!  NXLJ 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 

THIS  PROGRAM  HAS  BEEN  DEVELOPED  USING  THE  IMSL  LIBRARY 
AVAILABLE  IN  THE  COMPUTER  CENTER  OF  THE  NAVAL 
POSTGRADUATE  SCHOOL 

IMPLICIT  REAL *8  1A-H,0-Z) 

COMMON  FI  7,71 ,FS( 7 , 7) , GGGT ( 7 )  ,AK!7,2),R!2,2J, 

♦AKRKTI7, 7 ), OF  I  7, 7) , FT! 7,7) ,FSMKHT!  7,7}  ,DFT<7,7)  , 
♦FSMKH!7,7),H12,7) 

COMMON/KTR/N,NS,NPD 
DIMENSION  PFULL!7,7J,PSQR<7> 

DIMENSION  DDT ! 7 ) 

DIMENSION  J128),V(7»7) »P!28),UD!28) ,VO(7,7) , 

♦  VAR ( 105 ) , DRV ( 105 ) ,C I  24 1 ,WKl  10  3,9 ) , PD! 28  I 
DIMENSION  TMPI!7,7J , TMP2! 7, 7 ) , THP3 ( 7 , 7 ) 

EQUIVALENCE  (UlU.VAR!  l ) )  , !  V!  1 , l ) » VAR! 29) J  .  (  P  ( l )  , 

♦  VAR  178) ), !UD( 1) tORVIl) ) , (VO! I » 1 ) ,ORV ( 29) ) , ( PD! I ) , 

♦DRV ( 78) I 

N=ORDER  OF  THE  SYSTEM  MODEL 
NP*NUMBER  OF  POINTS 

NPD=»CONTROL  OF  INITIAL  DIAGNOSTIC  OUTPUT 

Q  T*  T I  ME  INTERVAL 

EXTERNAL  FUN 
CALL  UGET  10(3,5,6) 


THE  FOLLOWING  SECTION  READS  THE  SPECIFIED  INPUT 
MATRICES,F,F*,GQGT , K*H  AND  R 


READ! 5*98 )N,NP,NPD,OT 
98  FORMAT(3I5,F10.5) 

WRITE(6.97)N,NP,NPD,DT 
97  FORMAT! •  l ' ,3I5«LX,GI8.1 
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SEP  BO  J  A  MATALLANA  R. 


UNCLASSIFIED 


n 

n 

n 

n 

n 

n 

END 

U 

U 

U 

u 

U 

U 

L 

4  5  1 

noon 


NS*N*( N+l 1/2 
Nl»NS+N**2+i 
NV=2*NS+N**2 
99  FORMAT ( 8  F10 • 5 } 

00  l  I*l«N 

1  RE AD( 5  * 99  >  ( F { I , J ) , J*1 , N) 

CAUL  USWFMI *F*,i,F,7,N,N,ll 

00  2  1*1, N 

2  REA0(5,99)  (F S< I, Jl  ,J*L,N) 

CALL  USWFMI • FS» , 2, FS, 7  ,N, N, 1) 

REAO( 5 , 99 )  (GQGTtl) 

CALL  USWFVI'GQGT* ,4,GQGT,N,i,i) 

00  3  1*1, N 

3  READ(  5,99)  (  AKU  ,  J  >  ,  J=  1  *2 ) 

CALL  USWFMI 'K' ,1, AK,7,N,2,1 I 

DO  4  1-1,2 

4  REAQ(5*99  J  (H( I, J ) ,  J=l ,N) 

CALL  JSWFM<*H',l,H,2,2,N,i> 

OQ  5  1=1,2 

5  REAOt  5,99)  C  R ( I  *  J ) , J=l ,2) 

CALL  USWFMI  *R',i,R»2,2,2,l) 

00  6  1=1,105 

6  VAR(I l*0» 

DO  7  1*1, N 
DO  7  J*l , N 

7  DF(I,J>*FSU.JJ-F(  I  jJI 

CALL  USWFMI'OEL  F  •  ,  5 ,0F  ,7  ,N  ,N  ,  1 ) 

CALL  VMULFFIAK,*,N,2,2, 7,  2,  TMP1, 7,1  Eft) 

CALL  VMULFP( TMP1, AK,N,2 ,N,7,7 ,  AKRKT , 7 , IER » 

CALL  USWF  MI •  KRKT*  , 4* AKRKT »7»N»N*l) 

CALCULATE  THE  DIFFERENCE  BETWEEN  THE  DYNAMICS 
IMPLEMENTED  IN  THE  FILTER  AND  THE  PLANT,  DF=F*~F 

00  20  1*1 ,N 
DO  20  J*1,N 
0FT(1*JI*0F(I .0) 

20  FT  1 1 ,  J)  =F  1 1  •  J  J 

CALL  VTRANX(0FT,N,N,7) 

CALL  USWF  MI • DEL  FT •  ,6 , DFT ,7 ,N ,N , 1 > 

CALL  VMULFFl AK,H,N,2,N,7,2, TMP1,7,IER) 

DO  21  1*1,7 
DO  21  J*1 ,7 

FSMKHII,  J)*FS(1,  JI-TNPlf  I,J) 

21  FSMKHTI I, J)=FSMKH(  I ,J> 

CALL  USWFMI •  F  S-  KH ' , 5,F$MKH, 7, N, N , 1 ) 

CALL  VTRANXI F  SMKHT • N,N , 7) 

CALL  USWFM{  *  IFS-KHlT*»8*FSMKHT»7  ,N,N,1I 
T*0. 

T0L*1.0-5 

IN0*1 

L*0 

IFIN.EQ.7)  GO  TO  11 

DO  31  1*1, NS 
L*L  +  l 

31  VAR (L)*U(  I) 
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C 


C 


32 


33 

11 


30 

90 


10 


00  32  1=1  ,N 
00  32  J=i  ,H 
L*L  +  1 

VAR(L)*V{  It J) 

00  33  1*1, N 
L=L*1 

VAR  ( L) =P(  I) 

00  10  K= 1 »NP 
TEN0-FINAL  TIME 

TENC=K*0T 

DVERK  SUBROUTINE  FINOS  THE  SOLUTION  OF  THE  SYSTEM  OF 
DIFFERENTIAL  EQUATIONS 

CALL  DVERKINV.FJN, T,VAR,TEND,TOL, IND,C»135,WK,IER> 

IF ( IN0.LE.0.0R. IER .NE.O ) STOP 
CALL  VCVTSF ( VAR( Nl ) »N,  PFULL  ,7  ) 

CALCULATE  ANO  PRINT  THE  RMS  ESTIMATE  ERRORS 

DO  30  1=1 ,N 

PSQR(I)*0$QRT(PFJLL(  If  II  I 
WRITE! 6, 90) T, (PSQRI I) 1 1=1 ,N) 

FORMAT ( «0T=' ,F10.5t •  PSR=« ,  7315. 7) 

IF  DESIRED  PRINT  THE  COVARIANCE  MATRICES, P,U  AND  V 
CALL  USWSMI 'U'flfU, N.2) 

CALL  USWFMI 'V* ,l,VAR{NS+l) ,7,N,N,2) 

CALL  USWSM< »P', l,VAR(Ni),N,2> 

CONTINUE 

STOP 

END 
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SUBROUTINE  FUNHV, T, VAR, DRV) 

FCN  SUBROUTINE  IS  USED  FOR  EVALUATING  FUNCT I ONS ( I NPUT ) 


99 


15 


IMPLICIT  REAL*8  (A-H.O-Z) 

COMMON  F(  7, 7 )  »FS(  7, 7i , GQGT (7),AK(7,2),R(2,2)» 
♦AKRKT(7,7) v OF (7,7) , FT( 7 ,7 ) , FSMKHT ( 7 , 7) ,DFT (7, 7) , 
♦FSMKH(7,7> ,H( 2,7) 

COM MON/ KT  R/N, NS, NPD 

DIMENSION  U(28) ,V<7.7) , P( 28 ) , UD ( 28  )  ,VD<7,7) , 

♦VAR (105) , DRV( 1051 ,C(24) ,WK( 10 5 , 9) , PD < 28 ) 
DIMENSION  TMP i( 7 , 7 ) ,TMP2( 7, 7) ,TMP3( 7,7) 

L=0 

DO  1  1=1, NS 
L  =  L  +  l 

.  U(  I )=VAR( LI 

DO  2  1*1, N 
DO  2  J=1 , N 
L=L+l 

l  V( 1 , J)*VAR( L) 

DO  3  1=1, NS 
L=L  +  i 

l  PCI )=VAR(  L ) 

IFIT.EQ.O ) KT=NPD 
KT=KT+1 

IF  (KT.GE.5I  GO  TO  15 
WRI TE(6 ,99)  T 
FORMAT ( lOT=* ,025.15) 

CALL  USWSM( *  U  • , 1 , U , N, 2 ) 

CALL  USWFM( »V',ltV,7,NtN,2) 

CALL  USWSM{ 'P',l,V,N,2) 

CALL  VMULFS(F»U,N»N,7,TMPl,7) 

IF ( KT.LT. 5)  CALL  U SWFM I • FU* , 2 ,TMP l , 7 ,N ,N, 2 ) 

CALL  VMULSF(U,N,FT,N,7,TMP2,7) 

IFIKT.LT. 5)  CALL  USWFM ( *UFT « , 3 , TMP2 ,7,N , N, 2 ) 


1  =  1, N 
J=1 ,  N 

I,J)=TMPl(I,J)+TMP2II,J) 

1,1)  *TMP1(I, I )+GQGT( I) 

VCV  TFS ( TMP1 ,N , 7 ,U0) 

•  LT . 5)  CALL  USWSM( 'UDOT' ,4,UD,N,2) 

VMULFF (F  ,V,N,NfN,7»7,TMPl,7fIER) 

•  LT.  5)  CALL  USWFM(*FV* , 2 , TMP 1 , 7 ,N , N, 2 ) 
VMULFF(V,FSMKHT,N,N,N,7,7,TMP2,7,IEfU 

•  LT.5)  CALL  U  SWFM( '  V*  ( FS  -KH )T' ,10,TMP2,7,N,N,2) 
VMULSF(U,N,DFT  ,N, 7, TMP  3, 7) 

.LT.5)  CALL  USWFM(  'U*DFT • , 5 , TMP3, 7 ,N , N , 2 ) 


DO  5 
DO  4 
TMP  l( 

TMP1( 

CALL 
IF  (KT 
CALL 
IF  ( KT 
CALL 
IF  ( KT 
CALL 
IF  ( KT 

00  7  1=1, N 
DO  6  J-l.N 

VDI  I,  JI-TMPK  I  •  J  )  +TMP2  (  I ,  J)  ♦TMP3  (  I,  J) 

VU( I,  I)*VO( I, I ) -GQGT ( I ) 

IF ( KT.LT. 5)  CALL  USWFM ( • VDOT • , 4, V D, 7 ,N, N, 2 ) 

CALL  VMJLFSIfSMKH.P.N.NW.TMPI  .7) 

IFIKT.LT. 5)  CALL  .USWFM  ( *  I FS-KHJ P  * , 8, TMP1 , 7 ,N , N, 2 ) 
CALL  VMUL  SF(P,N,F  SMKHT »N,7,TMP2»7 ) 

IFIKT.LT. 5)  CALL  US WFM ( • P ( F S- KH ) T • . 9, TMP2 , 7 ,N  ,N ,2 ) 
CALL  VMULFF (DF,V»N»N»N»7,7» TMP3  »  7 , 1 ER) 

IF ( KT.LT. 5)  CALL  USWFM(,DF*V'  ,4 , TMP3 , 7 , N , N, 2 ) 

00  8  1*1, N 
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c 


c 

c 

c 


00  3  J*1,N 

3  TMP1II,  J)*TMP1(  I,  J)+TMP2(  I,  JM-TMP3I  I,J) 

CALL  VMULFMI V.0F,N,N,N, 7, 7, TMP3 , 7 , IER) 
IFTkT.LT. 5)  CALL  US WFM < • VT*DF • , 5 , TMP3 , 7 , M ,N ,2 > 

00  LO  1*1, N 
00  9  J*1,N 

9  TMP3I I  ,  Ji *TMPL< I , J ) +TMP3 ( I . J ) +AKRKT ( I , J ) 

LO  TMP3(I,I)*TMP3<I,I)«-GQGT(I) 

IF (KT .LT . 5 i  CALL  USWFM ( ' PDOT* , 4, TMP3 , 7 , * , N , 2 ) 
CALL  VCVTFS(TMP3»N,7,PD) 

IF ( KT .LT .5)  CALL  JSWFV { 'POOT{ SYMI • , 9,P0,S , 1 ,2  ) 
L*0 


00  1L  1*1, NS 
L*L+1 

LL  0RV(-Li*UDm 

00  12  1*1  ,N 
00  12  J*l,N 
L*L+i 

12  DRV (L)*VD( I  ,  J  ) 

00  13  1*1, NS 
L*L+1 

13  ORV (L)*PD( I ) 

IFIKT.LT. 5)  CALL  USWFV ( *ORV * , 3, DRV,NV, 1, 2 ) 

RETURN 

END 
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